﻿#include "stackcommon.h"

#include "../algorithm_log.h"
#include "commonfunctionset.h"

#define JIG_X (230)
#define JIG_Y (405)

using namespace stack;
StackCommon::StackCommon()
{}

StackCommon::~StackCommon()
{}

json_struct(StackCfgInput,
	mapping(int, dirOdd);
mapping(int, dirEven);
prop(&dirOdd, &dirEven);
);

bool StackCommon::Init(const AlgInit& init, const QString& other)
{
	m_plate = init.base;
	m_plate.c = 0;
	m_plate.b = 0;
	m_plateRang = init.rang;

	m_shap = new StackShapManager;
	if (m_shap->Init() <= 0) {
		log_error("stack shap manager init failed");
		return false;
	}
	StackCfgInput cfg = {};
	cfg.fromJson(other.toUtf8());
	m_dirEven = (StackDir)cfg.dirEven();
	m_dirOdd = (StackDir)cfg.dirOdd();
	if (m_dirEven >= StackDir::stack_dir_max || m_dirEven < StackDir::stack_dir_first) {
		log_error("dir error");
		return false;
	}
	if (m_dirOdd >= StackDir::stack_dir_max || m_dirOdd < StackDir::stack_dir_first) {
		log_error("dir error");
		return false;
	}

	return true;
}


json_struct(AlgStackInfo,
	mapping(int, id);
mapping(QString, name);
mapping(int, ceng);
mapping(int, max);
prop(&id, &name, &ceng, &max);
);
bool StackCommon::InitSkuInfo(const AlgSkuInfo& sku, QString& result)
{
	m_stackInfo = { 0 };
	m_sku = sku.size;
	if (sku.stackType == 0) {
		m_stackInfo = m_shap->ChooseStackForSku(m_sku, m_plateRang);
		if (m_stackInfo.id <= 0) {
			result = "未匹配到合适垛形";
			return false;
		}
	}
	log_trace("stack type = {}， name = {}", m_stackInfo.id, m_stackInfo.name);
	oddpos.clear();
	evenpos.clear();
	m_oddpos.clear();
	m_evenpos.clear();
	m_shap->GetSkuPosition(m_stackInfo.id, m_sku, true, oddpos);
	m_shap->GetSkuPosition(m_stackInfo.id, m_sku, false, evenpos);

	if (oddpos.size() <= 0 || evenpos.size() <= 0) {
		log_error("Get stack sku pos failed");
		result = "垛形信息异常";
		return false;
	}

	if (!SortSkuPos(oddpos, m_dirOdd)) {
		log_error("Not cfg Odd order");
		return false;
	}
	auto groupfunc = [](const stack::SkuPosInfo& a, const stack::SkuPosInfo& b) {
		return a.group < b.group;
	};

	std::sort(oddpos.begin(), oddpos.end(), groupfunc);
	std::sort(evenpos.begin(), evenpos.end(), groupfunc);
	QVector<SkuPosInfo> list;
	for (auto& iter : oddpos) {
		if (list.size() > 0) {
			if (list.at(0).group == iter.group) {
				list.append(iter);
			} else {
				SortSkuPos(list, m_dirOdd);
				m_oddpos.append(list);
				list.clear();
				list.append(iter);
			}
		} else {
			list.append(iter);
		}
	}
	m_oddpos.append(list);
	list.clear();
	for (auto& iter : evenpos) {
		if (list.size() > 0) {
			if (list.at(0).group == iter.group) {
				list.append(iter);
			} else {
				SortSkuPos(list, m_dirEven);
				m_evenpos.append(list);
				list.clear();
				list.append(iter);
			}
		} else {
			list.append(iter);
		}
	}
	m_evenpos.append(list);

	SortSkuGroupPos(m_oddpos, m_dirOdd);
	SortSkuGroupPos(m_evenpos, m_dirEven);
	AlgStackInfo info = {};
	info.ceng = m_stackInfo.cengNum;
	info.id = m_stackInfo.id;
	info.name = m_stackInfo.name;
	info.max = m_stackInfo.maxNum;
	result = info.toByteArray();
	return true;
}

json_struct(AlgCalParam,
	mapping(int, num);
mapping(int, grabed);
prop(&num, &grabed);
);
/*算法结果：本次的最大抓取数量*/
json_struct(StackAlgResult,
	mapping(int, num);
prop(&num);
);

bool StackCommon::Calculate(const QString& input, Position& pos, QString& output)
{
	if (m_stackInfo.id <= 0) {
		output = "物料垛形未初始化";
		return false;
	}

	AlgCalParam in = {};
	in.fromJson(input.toUtf8());
	int num = GetNextGrabNum(in.grabed());
	log_trace("input num = {}, get next num = {}, grabed num = {}", in.num(), num, in.grabed());
	if (in.num() > num || in.num() <= 0) {
		output = QString("输入的抓取数量%1异常").arg(in.num());
		return false;
	}
	Position skuInPlate = {};
	bool ret = GetGrabSkuPosition(in.grabed(), in.num(), skuInPlate);
	if (!ret) {
		output = "获取物料在托盘上的坐标出错";
		return false;
	}
	log_trace("sku in plate pos = ({}, {}, {}, {})", skuInPlate.x, skuInPlate.y, skuInPlate.z, skuInPlate.a);
	Position skuInWord = CommonFun::TransAToBCoordinateSystem(skuInPlate, m_plate);
	log_trace("sku in word pos = ({}, {}, {}, {})", skuInWord.x, skuInWord.y, skuInWord.z, skuInWord.a);

	Position jigInSKu = { m_sku.length / 2 - JIG_X,JIG_Y - m_sku.width * num / 2,0,0,0,180 };
	pos = CommonFun::TransAToBCoordinateSystem(jigInSKu, skuInWord);
	log_trace("jig in word pos = ({}, {}, {}, {})", pos.x, pos.y, pos.z, pos.a);
	return true;
}

bool StackCommon::GetGrabSkuPosition(int grabed, int grabnum, Position& out)
{
	int ceng = grabed / m_stackInfo.cengNum + 1;		// 当前抓取的层数
	int index = grabed % m_stackInfo.cengNum;			// 当前层已经抓了几个
	Position total = {};
	if (ceng % 2 == 1) {	// 奇数层
		int num = 0;
		for (auto& iter : m_oddpos) {
			if (num + iter.size() > index) {
				log_trace("get group ={}", iter.at(0).group);
				for (int i = index - num; i < grabnum + index - num; i++) {
					auto& tem = iter[i];
					total.x += tem.x;
					total.y += tem.y;
					total.a += tem.za;
					log_trace("sku id = {}", tem.id);
				}
				break;
			} else {
				num += iter.size();
			}
		}
	} else {
		int num = 0;
		for (auto& iter : m_evenpos) {
			if (num + iter.size() > index) {
				log_trace("get group ={}", iter.at(0).group);
				for (int i = index - num; i < grabnum + index - num; i++) {
					auto& tem = iter[i];
					total.x += tem.x;
					total.y += tem.y;
					total.a += tem.za;
					log_trace("sku id = {}", tem.id);
				}
				break;
			} else {
				num += iter.size();
			}
		}
	}
	int za = total.a / grabnum;
	if (za % 90 != 0) {
		log_error("Sku in plate za is not 90, za = {}", za);
		return false;
	}
	out.x = total.x / grabnum;
	out.y = total.y / grabnum;
	out.z = ceng * m_sku.height;
	out.a = total.a / grabnum;
	out.b = 0;
	out.c = 0;
	return true;
}


int StackCommon::GetNextGrabNum(int grabed)
{
	log_trace("GetNextGrabNum grabed {},  {}, {} ", grabed, oddpos.size(), evenpos.size());
	if (grabed > m_stackInfo.maxNum) {
		return -1;
	}

	int ceng = grabed / m_stackInfo.cengNum + 1;		// 当前抓取的层数
	int index = grabed % m_stackInfo.cengNum;			// 当前层已经抓了几个
	if (ceng % 2 == 1) {	// 奇数层
		int num = 0;
		for (auto& iter : m_oddpos) {
			if (num + iter.size() > index) {
				log_trace("get group ={}", iter.at(0).group);
				return iter.size() + num - index;
			} else {
				num += iter.size();
			}
		}
		return -1;
	} else {
		int num = 0;
		for (auto& iter : m_evenpos) {
			if (num + iter.size() > index) {
				return iter.size() + num - index;
			} else {
				num += iter.size();
			}
		}
		return -1;
	}
}

bool StackCommon::ExcuteCmd(int cmd, const QString& input, QString& output)
{
	if (cmd == 1) {	// 获取下一次抓取数量
		AlgCalParam in = {};
		in.fromJson(input.toUtf8());
		StackAlgResult out = {};
		out.num = GetNextGrabNum(in.grabed());
		output = out.toByteArray();
		return true;
	}

	return false;
}

bool StackCommon::SortSkuPos(QVector<stack::SkuPosInfo>& pos, StackDir type)
{
	// x由小变大，y由小变大
	auto fun1 = [](const SkuPosInfo& p1, const SkuPosInfo& p2) {
		int num2 = p2.x + p2.y * 1;
		int num1 = p1.x + p1.y * 1;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.y < p2.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由小变大
	auto fun2 = [](const SkuPosInfo& p1, const SkuPosInfo& p2) {
		int num2 = -p2.x + p2.y * 1;
		int num1 = -p1.x + p1.y * 1;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.y < p2.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	auto fun3 = [](const SkuPosInfo& p1, const SkuPosInfo& p2) {
		int num2 = p2.x - p2.y * 1;
		int num1 = p1.x - p1.y * 1;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.y > p2.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	auto fun4 = [](const SkuPosInfo& p1, const SkuPosInfo& p2) {
		int num2 = -p2.x - p2.y * 1;
		int num1 = -p1.x - p1.y * 1;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.y > p2.y) {
				return true;
			} else {
				return false;
			}
		}
	};

	switch (type) {
	case StackDir::stack_dir_first:
		std::sort(pos.begin(), pos.end(), fun4);
		break;
	case StackDir::stack_dir_second:
		std::sort(pos.begin(), pos.end(), fun3);
		break;
	case StackDir::stack_dir_third:
		std::sort(pos.begin(), pos.end(), fun1);
		break;
	case StackDir::stack_dir_foruth:
		std::sort(pos.begin(), pos.end(), fun2);
		break;
	default:
		return false;
		break;
	}

	return true;

}
